#include "TransientModalSolver.h"

#include "Utility/Algebra/Factory.h"
#include "Utility/Algebra/TimeSteppers.h"
#include "Utility/Algebra/sgArpack.h"
#include "Utility/Configuration/SGParameter.h"
#include "Utility/Logging/SGLogger.h"

#include "DataStructure/Input/Solver/Subcase.h"

#include "DBManager/IService/Input/Dynamic/IEIGRLService.h"
#include "DBManager/IService/Input/Subcase/ISubcaseService.h"

#include "SGFem/Pre/Assembly/Assembly.h"

#include "SGFem/Task/Kernel/MModel.h"
#include "SGFem/Task/Kernel/MTRAModel.h"

using SG::DataStructure::Common::Status;
using SG::DataStructure::FEM::InitConditonType;
using SG::DataStructure::FEM::Project;
using SG::DataStructure::FEM::SOLType;
using namespace SG::DBManager;

/**
 * @brief 杜阿梅尔积分计算 带阻尼的振动系统二阶微分方程
    控制方程:
        m * ξ''(t) + b * ξ'(t) + k * ξ(t) = p(t)

    系统解析解（考虑初始条件与外力）:
    ξ(t) = e^(-b*t/(2m)) * [ ξ0 * cos(ω_d * t) + (ξ0 * b/(2m) + ξ0_dot) / ω_d * sin(ω_d * t) ]
           + (1 / (m * ω_d)) * ∫₀ᵗ e^(-b*(t - τ)/(2m)) * p(τ) * sin[ω_d * (t - τ)] dτ

    变量说明:
    u0      = initial displacement             // 初始位移
    v0      = initial velocity                 // 初始速度
    m       = mass                             // 质量
    b       = damping coefficient              // 阻尼
    k       = stiffness                        // 刚度
    p(t)    = external excitation force        // 载荷

 *
 * @param    u0      初始位移
 * @param    v0      初始速度
 * @param    m         质量
 * @param    b         阻尼
 * @param    k         刚度
 * @param    p         待求解时刻的载荷向量
 * @param    times        时间向量
 * @return
 * @author   wjw
 * @date     2025.07.01
 */
SG::Algebra::Vec_t ComputeDuhamelIntegralVector (SG::Algebra::Real_t       u0,
                                                 SG::Algebra::Real_t       v0,
                                                 SG::Algebra::Real_t       m,
                                                 SG::Algebra::Real_t       b,
                                                 SG::Algebra::Real_t       k,
                                                 const SG::Algebra::Vec_t& p,
                                                 const SG::Algebra::Vec_t& t)
{
    // 参数
    SG::Algebra::Real_t omega  = sqrt (k / m);              // 圆频率
    SG::Algebra::Real_t Xi     = b / (2.0 * sqrt (k * m));  // 临界阻尼比
    SG::Algebra::Real_t omegad = omega * sqrt (1.0 - Xi * Xi);
    SG::Algebra::Vec_t  displacement (t.size ());

    displacement (0) = u0;

    // 预计算所有时间点的指数和三角函数值
    SG::Algebra::Vec_t expTemp = (-b * 0.5 / m * t.array ()).exp ();
    SG::Algebra::Vec_t cosTemp = (omegad * t.array ()).cos ();
    SG::Algebra::Vec_t sinTemp = (omegad * t.array ()).sin ();

    // 齐次解
    SG::Algebra::Vec_t homogeneous = expTemp.cwiseProduct (u0 * cosTemp + (v0 + b * 0.5 / m * u0) / omegad * sinTemp);

    // 预计算时间步长
    SG::Algebra::Vec_t dt = SG::Algebra::Vec_t::Zero (t.size () - 1);
    for (int i = 0; i < t.size () - 1; ++i)
    {
        dt (i) = t (i + 1) - t (i);
    }

    // 卷积积分
    for (int j = 1; j < t.size (); ++j)
    {
        // 创建时间差矩阵的一行
        SG::Algebra::Vec_t tau_diff = SG::Algebra::Vec_t::Constant (j, t (j)) - t.head (j);

        // 被积函数
        SG::Algebra::Vec_t h_values =
            1.0 / (m * omegad)
            * (-b * 0.5 / m * tau_diff.array ()).exp ().matrix ().cwiseProduct ((omegad * tau_diff.array ()).sin ().matrix ());
        SG::Algebra::Vec_t f_values = p.head (j).cwiseProduct (h_values);

        // 梯形积分
        SG::Algebra::Real_t integral = 0.0;
        if (j > 1)
        {
            SG::Algebra::Vec_t mid_values = 0.5 * (f_values.head (j - 1) + f_values.segment (1, j - 1));
            integral += mid_values.cwiseProduct (dt.head (j - 1)).sum ();
        }

        // 处理最后一段
        SG::Algebra::Real_t dt_last = t (j) - t (j - 1);
        integral += f_values (j - 1) * dt_last;

        displacement (j) = homogeneous (j) + integral;
    }

    return displacement;
}

void TransientModalSolver::Solve (const DBServiceFactorySP dbServiceFactory, const Project& project, int subcaseId)
{
    Kernel::MModel modal (SOLType::SOL_SEMTRAN, dbServiceFactory);
    modal.Initialize ({ subcaseId });
    modal.CalcMatrixEquation (subcaseId);

    auto                            para = dbServiceFactory->get<ISubcaseService> ()->FindSubCase (subcaseId);
    SG::DataStructure::FEM::Subcase subcase (para);

    SG::DataStructure::FEM::EIGRLData EIGRLData;
    dbServiceFactory->get<IEIGRLService> ()->FindBySId (subcase.GetMethodId (), EIGRLData);
    auto        nModes   = EIGRLData.m_ND;
    auto&       assembly = SG::FEMSolver::AssemblyEigen::GetInstance ();
    std::size_t n        = assembly.GetFreeDOFSize ();

    // 获取解特征值和特征向量
    SG::Algebra::Mat_t phi (n, nModes);
    SG::Algebra::Vec_t eigen (nModes);
    modal.ComputeEigenValue (nModes, eigen, phi);

    // 获取模态空间初始条件
    SG::Algebra::Vec_t xi_0 (nModes);
    SG::Algebra::Vec_t xi_dot_0 (nModes);
    {
        InitConditonType   type;
        auto               initId = subcase.GetICId (type);
        SG::Algebra::Vec_t U0 (n), V0 (n);
        modal.ComputeInitialCondition (initId, type, U0, V0);
        // 0 时刻位移存储
        modal.SetDisplacement (U0, subcaseId, 0.0);
        xi_0     = phi.transpose () * U0;
        xi_dot_0 = phi.transpose () * V0;
    }

    // 获取模态阻尼矩阵
    SG::Algebra::SpMat_t C;
    modal.ComputeDampingMatrix (C);
    SG::Algebra::Mat_t modalC = phi.transpose () * C * phi;

    // 计算静载荷
    modal.ComputeFValue (subcaseId);

    // 获取时间向量
    std::vector<SG::DataStructure::Common::Real> times;
    std::vector<SG::DataStructure::Common::Real> tOut;
    modal.QueryTimeStep (subcase.GetTSTEPId (), times, tOut);

    // 判断模态阻尼阵是否为对角阵
    bool isDiagonal = modalC.isDiagonal (SG::DataStructure::Common::TOLERANCE);
    if (isDiagonal)
    {
        // 计算模态载荷
        SG::Algebra::Mat_t fi (nModes, times.size ());
        {
            SG::Algebra::Mat_t F (n, times.size ());
            SG::Algebra::Vec_t U_k (n), V_k (n), F_kp1 (n);

            U_k.setZero ();
            V_k.setZero ();

            for (SG::Algebra::Real_t i = 0; i < times.size (); ++i)
            {
                modal.ComputeExplicitValue (times[i], U_k, V_k, F_kp1);
                F.col (i) = F_kp1;
            }

            fi = phi.transpose () * F;
        }

        // 计算每个模态的参数和响应
        const auto                      nStep = times.size ();
        std::vector<SG::Algebra::Vec_t> uMode (nStep, SG::Algebra::Vec_t (nModes));
        for (int i = 0; i < nModes; ++i)
        {
            // 模态参数
            SG::Algebra::Real_t m       = 1.0;       // 归一化质量
            SG::Algebra::Real_t k       = eigen[i];  // 刚度
            SG::Algebra::Real_t omega_n = sqrt (eigen[i]);
            SG::Algebra::Real_t f       = omega_n / (2 * M_PI);
            SG::Algebra::Real_t ci      = modal.ComputeDampingRatio (f) + modalC (i, i) / (2.0 * omega_n * m);
            SG::Algebra::Real_t b       = 2.0 * ci * omega_n;  // 阻尼

            // 提取该模态的载荷向量
            SG::Algebra::Vec_t p_modal = fi.row (i);

            // 计算第i阶模态方向的的位移响应
            SG::Algebra::Vec_t t   = SG::Algebra::Vec_t::Map (times.data (), times.size ());
            auto               umI = ComputeDuhamelIntegralVector (xi_0[i], xi_dot_0[i], m, b, k, p_modal, t);

            // 存储该模态的响应
            for (std::size_t t = 0; t < nStep; ++t)
            {
                uMode[t](i) = umI (t);
            }
        }

        // 单独的时间循环用于存储物理位移
        for (std::size_t t = 0; t < times.size (); ++t)
        {
            // 恢复物理空间位移向量并保存
            modal.SetDisplacement (phi * uMode[t], subcaseId, times[t]);
        }
    }
    else
    {
        // 阻尼阵非解耦的情况保持不变
        SG::Algebra::Vec_t Xi (nModes), Xi_dot (nModes), Xi_double_dot (nModes);
        Xi     = xi_0;
        Xi_dot = xi_dot_0;

        Kernel::MTRAModel MTRAModel (modal, phi, eigen);
        void*             solver = SG::Base::Factory::GetInstance ()->Produce ("ModifiedCentralDifference");
        auto              Solver = reinterpret_cast<SG::Algebra::SecondOrderTimeStepper*> (solver);
        Solver->Initialize (&MTRAModel);

        for (std::size_t i = 1; i < times.size (); ++i)
        {
            Solver->SetTime (times[i]);
            Solver->SetTimeStep (times[i] - times[i - 1]);
            Solver->Step (Xi, Xi_dot, Xi_double_dot);

            modal.SetDisplacement (phi * Xi, subcaseId, times[i]);
        }
    }

    SGInfo << "TransientModalAnalysis Solve fini  \n";
}